from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription, TimerAction, RegisterEventHandler, LogInfo
from launch.event_handlers import OnProcessExit, OnProcessIO
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
import os

def generate_launch_description():
    # UR机械臂控制节点
    ur_control_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('ur_robotiq_moveit_config'),
                        'launch/ur_robotiq_control.launch.py')
        ]),
        launch_arguments={
            'use_fake_hardware': 'true',
            'ur_type': 'ur5',
            'robot_ip': '192.168.0.101',
            'launch_rviz': 'false'
        }.items()
    )

    # MoveIt节点
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('ur_robotiq_moveit_config'),
                        'launch/ur_robotiq_moveit.launch.py')
        ]),
        launch_arguments={
            'use_fake_hardware': 'false',
            'ur_type': 'ur5',
            'launch_rviz': 'true'
        }.items()
    )

    # Astra相机节点
    camera_launch = ExecuteProcess(
        cmd=[
            'agiros', 'launch', 'astra_camera',
            'astra.launch.xml',
        ],
        output='screen',
        name='camera_launch_process'
    )

    # Aruco识别节点
    aruco_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('ur5_moveit_demo'),
                        'launch/single.launch.py')
        ]),
        launch_arguments={
            'marker_id': '582',
            'marker_size': '0.05',
            'camera_frame': 'camera_link',
            'camera_info_topic': '/camera/color/camera_info',
            'image_topic': '/camera/color/image_raw',
            'marker_frame': 'aruco_marker_frame',
            'reference_frame': 'camera_link',
            'corner_refinement': 'LINES'
        }.items()
    )

    # easy_handeye2标定节点
    handeye_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('easy_handeye2'),
                        'launch/publish.launch.py')
        ]),
        launch_arguments={
            'name': 'eye2hand_camera'
        }.items()
    )

    # RQT可视化节点
    rqt_process = ExecuteProcess(
        cmd=['rqt', '--perspective-file', 
             os.path.join(get_package_share_directory('ur5_moveit_demo'),
                         'config/aruco.perspective')],
        output='screen'
    )


    return LaunchDescription([
        ur_control_launch,
        moveit_launch,
        camera_launch,
        aruco_launch,
        handeye_launch,
        rqt_process
        ])
